//////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2010, Jason Mora Saragih, all rights reserved.
//
// This file is part of FaceTracker.
//
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions are met:
//
//     * The software is provided under the terms of this licence stricly for
//       academic, non-commercial, not-for-profit purposes.
//     * Redistributions of source code must retain the above copyright notice, 
//       this list of conditions (licence) and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright 
//       notice, this list of conditions (licence) and the following disclaimer 
//       in the documentation and/or other materials provided with the 
//       distribution.
//     * The name of the author may not be used to endorse or promote products 
//       derived from this software without specific prior written permission.
//     * As this software depends on other libraries, the user must adhere to 
//       and keep in place any licencing terms of those libraries.
//     * Any publications arising from the use of this software, including but
//       not limited to academic journal and conference publications, technical
//       reports and manuals, must cite the following work:
//
//       J. M. Saragih, S. Lucey, and J. F. Cohn. Face Alignment through 
//       Subspace Constrained Mean-Shifts. International Conference of Computer 
//       Vision (ICCV), September, 2009.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED 
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 
// EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
///////////////////////////////////////////////////////////////////////////////
#include "Tracker.h"
#include "classify.h"
#include <opencv/highgui.h>

#include <iostream>

#include <termios.h>
#include <unistd.h>


using namespace std;

static const bool jenComputer = false;

static string databaseFile = !jenComputer 
  ?  "/home/mfleder/Documents/Spring_2012_Classes/6.857/faceRecognition/FaceTracker/src/exe/data/training_vectors.txt"
  : "/Users/jenlwong/Documents/Spring 2012/6.857/Project/FaceTracker/src/face-recognition-6857/data/training_vectors.txt";
                     

static string modelDirectory = jenComputer 
  ? "/Users/jenlwong/Documents/Spring 2012/6.857/Project/FaceTracker/model/"
  : "../model/";


//=============================================================================
void Draw(cv::Mat &image,cv::Mat &shape,cv::Mat &con,cv::Mat &tri,cv::Mat &visi)
{
  int i,n = shape.rows/2; cv::Point p1,p2; cv::Scalar c;

  //draw triangulation
  c = CV_RGB(0,0,0);
  for(i = 0; i < tri.rows; i++){
    if(visi.at<int>(tri.at<int>(i,0),0) == 0 ||
       visi.at<int>(tri.at<int>(i,1),0) == 0 ||
       visi.at<int>(tri.at<int>(i,2),0) == 0)continue;
    p1 = cv::Point(shape.at<double>(tri.at<int>(i,0),0),
		   shape.at<double>(tri.at<int>(i,0)+n,0));
    p2 = cv::Point(shape.at<double>(tri.at<int>(i,1),0),
		   shape.at<double>(tri.at<int>(i,1)+n,0));
    cv::line(image,p1,p2,c);
    p1 = cv::Point(shape.at<double>(tri.at<int>(i,0),0),
		   shape.at<double>(tri.at<int>(i,0)+n,0));
    p2 = cv::Point(shape.at<double>(tri.at<int>(i,2),0),
		   shape.at<double>(tri.at<int>(i,2)+n,0));
    cv::line(image,p1,p2,c);
    p1 = cv::Point(shape.at<double>(tri.at<int>(i,2),0),
		   shape.at<double>(tri.at<int>(i,2)+n,0));
    p2 = cv::Point(shape.at<double>(tri.at<int>(i,1),0),
		   shape.at<double>(tri.at<int>(i,1)+n,0));
    cv::line(image,p1,p2,c);
  }
  //draw connections
  c = CV_RGB(0,0,255);
  for(i = 0; i < con.cols; i++){
    if(visi.at<int>(con.at<int>(0,i),0) == 0 ||
       visi.at<int>(con.at<int>(1,i),0) == 0)continue;
    p1 = cv::Point(shape.at<double>(con.at<int>(0,i),0),
		   shape.at<double>(con.at<int>(0,i)+n,0));
    p2 = cv::Point(shape.at<double>(con.at<int>(1,i),0),
		   shape.at<double>(con.at<int>(1,i)+n,0));
    cv::line(image,p1,p2,c,1);
  }
  //draw points
  for(i = 0; i < n; i++){    
    if(visi.at<int>(i,0) == 0)continue;
    p1 = cv::Point(shape.at<double>(i,0),shape.at<double>(i+n,0));
    c = CV_RGB(255,0,0); cv::circle(image,p1,2,c);
  }return;
}
//=============================================================================
int parse_cmd(int argc, const char** argv,
	      char* ftFile,char* conFile,char* triFile,
	      bool &fcheck,double &scale,int &fpd)
{
  int i; fcheck = false; scale = 1; fpd = -1;
  for(i = 1; i < argc; i++){
    if((std::strcmp(argv[i],"-?") == 0) ||
       (std::strcmp(argv[i],"--help") == 0)){
      std::cout << "track_face:- Written by Jason Saragih 2010" << std::endl
	   << "Performs automatic face tracking" << std::endl << std::endl
	   << "#" << std::endl 
	   << "# usage: ./face_tracker [options]" << std::endl
	   << "#" << std::endl << std::endl
	   << "Arguments:" << std::endl
	   << "-m <string> -> Tracker model (default: ../model/face2.tracker)"
	   << std::endl
	   << "-c <string> -> Connectivity (default: ../model/face.con)"
	   << std::endl
	   << "-t <string> -> Triangulation (default: ../model/face.tri)"
	   << std::endl
	   << "-s <double> -> Image scaling (default: 1)" << std::endl
	   << "-d <int>    -> Frames/detections (default: -1)" << std::endl
	   << "--check     -> Check for failure" << std::endl;
      return -1;
    }
  }
  for(i = 1; i < argc; i++){
    if(std::strcmp(argv[i],"--check") == 0){fcheck = true; break;}
  }
  if(i >= argc)
    fcheck = false;
  for(i = 1; i < argc; i++)
    {
    if(std::strcmp(argv[i],"-s") == 0)
      {
	if(argc > i+1)
	  scale = std::atof(argv[i+1]); 
	else scale = 1;
      break;
    }
  }
  if(i >= argc)scale = 1;
  for(i = 1; i < argc; i++){
    if(std::strcmp(argv[i],"-d") == 0){
      if(argc > i+1)fpd = std::atoi(argv[i+1]); else fpd = -1;
      break;
    }
  }
  if(i >= argc)fpd = -1;
  for(i = 1; i < argc; i++){
    if(std::strcmp(argv[i],"-m") == 0){
      if(argc > i+1)std::strcpy(ftFile,argv[i+1]);
      else strcpy(ftFile, (modelDirectory + "face2.tracker").c_str());
      break;
    }
  }
  if(i >= argc)std::strcpy(ftFile, (modelDirectory + "face2.tracker").c_str());
  for(i = 1; i < argc; i++){
    if(std::strcmp(argv[i],"-c") == 0){
      if(argc > i+1)std::strcpy(conFile,argv[i+1]);
      else strcpy(conFile, (modelDirectory + "face.con").c_str());
      break;
    }
  }
  if(i >= argc)std::strcpy(conFile, (modelDirectory + "face.con").c_str());
  for(i = 1; i < argc; i++){
    if(std::strcmp(argv[i],"-t") == 0){
      if(argc > i+1)std::strcpy(triFile,argv[i+1]);
      else strcpy(triFile, (modelDirectory + "face.tri").c_str());
      break;
    }
  }
  if(i >= argc)std::strcpy(triFile, (modelDirectory + "face.tri").c_str());
  return 0;
}

void addToDB(std::vector<double> feature_vector, std::string filename, std::string userID) {
    std::ofstream training_set_file;
    training_set_file.open(filename.c_str(), std::ios_base::app);
    training_set_file << "\n" << userID << ": ";
    for (int i = 0; i < feature_vector.size() - 1; i++) {
        training_set_file << feature_vector[i] << " ";
    }
    training_set_file << feature_vector[feature_vector.size() - 1];
    training_set_file.close();
}

//==================
std::vector<double> generateFeatureVector(cv::Mat &shape, cv::Mat &visi) 
{
    int n = shape.rows/2;
    std::vector<double> feature_vector;
    for(int i = 0; i < n; i++){    
        if(visi.at<int>(i,0) == 0)continue;
        feature_vector.push_back(shape.at<double>(i,0));
        feature_vector.push_back(shape.at<double>(i+n,0)); 
    }
    
    double x_min = 10e16;
    double x_max = 10e-16;
    double y_min = 10e16;
    double y_max = 10e-16;
    
    for (int i = 0; i < feature_vector.size(); i += 2) {
        if (feature_vector[i] < x_min) {
            x_min = feature_vector[i];
        }
        if (feature_vector[i] > x_max) {
            x_max = feature_vector[i];
        }
        if (feature_vector[i+1] < y_min) {
            y_min = feature_vector[i+1];
        }
        if (feature_vector[i+1] > y_max) {
            y_max = feature_vector[i+1];
        }
    }
    
    for (int i = 0; i < feature_vector.size(); i++) {
        if (i % 2 == 0) {
            feature_vector[i] = feature_vector[i] - (x_min + x_max)/2;             
        } else {
            feature_vector[i] = feature_vector[i] - (y_min + y_max)/2; 
        }
    }
    
    if ((x_max - (x_min+x_max)/2) > (y_max - (y_min+y_max)/2)) {
        for (int i = 0; i < feature_vector.size(); i++) {
            feature_vector[i] = feature_vector[i] / (x_max - (x_min+x_max)/2);
        }
    } else {
        for (int i = 0; i < feature_vector.size(); i++) {
            feature_vector[i] = feature_vector[i] / (y_max - (y_min+y_max)/2);
        }   
        
    }
    
    return feature_vector;
    
}


void getPictureFromFile(cv::Mat &shape, cv::Mat &visi, std::string filename,
			 int argc, const char**argv)
{
  //By : Seo Jin
  //TODO: use command line argument
    char ftFile[256],conFile[256],triFile[256];
    strcpy(ftFile,"../model/face2.tracker");
    strcpy(conFile,"../model/face.con");
    strcpy(triFile,"../model/face.tri");
    bool fcheck = false; double scale = 1; int fpd = -1; bool show = true;

    //set other tracking parameters
    std::vector<int> wSize1(1); wSize1[0] = 7;
    std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7;
    int nIter = 5; double clamp=3,fTol=0.01; 

    FACETRACKER::Tracker model(ftFile);
    cv::Mat tri=FACETRACKER::IO::LoadTri(triFile);
    cv::Mat con=FACETRACKER::IO::LoadCon(conFile);
    cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text; 
    bool failed = true;

    IplImage* I = cvLoadImage(filename.c_str()); frame = I;
    if(scale == 1)im = frame; 
    else cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows));
    cv::flip(im,im,1); cv::cvtColor(im,gray,CV_BGR2GRAY);

    //track this image
    std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1; 
    std::vector<double> fv;
    if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0){
      int idx = model._clm.GetViewIdx(); failed = false;
      shape = model._shape;
      visi = model._clm._visi[idx];
    }
}

void getPictureFromWebcam(cv::Mat &shape, cv::Mat &visi, 
			  int argc, const char**argv)
{
  //parse command line arguments
  char ftFile[256],conFile[256],triFile[256];
  bool fcheck = false; double scale = 1; int fpd = -1; bool show = true;
  if(parse_cmd(argc,argv,ftFile,conFile,triFile,fcheck,scale,fpd)<0)
    return;

  //set other tracking parameters
  std::vector<int> wSize1(1); wSize1[0] = 7;
  std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7;
  int nIter = 5; double clamp=3,fTol=0.01; 
  FACETRACKER::Tracker model(ftFile);
  cv::Mat tri=FACETRACKER::IO::LoadTri(triFile);
  cv::Mat con=FACETRACKER::IO::LoadCon(conFile);
  
  //initialize camera and display window
  cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text; 
  CvCapture* camera = cvCreateCameraCapture(CV_CAP_ANY); 
  if(!camera)
    return;

  int64 t1,t0 = cvGetTickCount(); int fnum=0;
  cvNamedWindow("Face Tracker",1);
  std::cout << "Hot keys: "        << std::endl
	    << "\t ESC - quit"     << std::endl
	    << "\t d   - Redetect" << std::endl
	    << "\t a - accept current face capture" << std::endl;

  //loop until quit (i.e user presses ESC)
  bool failed = true;
  while(1)
    { 
      //grab image, resize and flip
      IplImage* I = cvQueryFrame(camera); if(!I)continue; frame = I;
      if(scale == 1)
	im = frame; 
      else 
	cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows));

      cv::flip(im,im,1); cv::cvtColor(im,gray,CV_BGR2GRAY);
      
      //track this image
      std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1; 
      if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0)
	{
	  int idx = model._clm.GetViewIdx(); failed = false;
	  Draw(im,model._shape,con,tri,model._clm._visi[idx]); 
	  shape = model._shape; //return arguments
	  visi = model._clm._visi[idx]; //return arguments
	}
      else
	{
	  if(show){cv::Mat R(im,cvRect(0,0,150,50)); R = cv::Scalar(0,0,255);}
	  model.FrameReset(); failed = true;
	}     
      //draw framerate on display image 
      if(fnum >= 9)
	{      
	  t1 = cvGetTickCount();
	  fps = 10.0/((double(t1-t0)/cvGetTickFrequency())/1e+6); 
	  t0 = t1; fnum = 0;
	}
      else 
	fnum += 1;
      if(show)
	{
	  sprintf(sss,"%d frames/sec",(int)round(fps)); text = sss;
	  cv::putText(im,text,cv::Point(10,20),
		      CV_FONT_HERSHEY_SIMPLEX,0.5,CV_RGB(255,255,255));
	}
      //show image and check for user input
      imshow("Face Tracker",im); 
      int c = cvWaitKey(10);
      if(c == 27)
	break;
      else if(char(c) == 'd')
	model.FrameReset();
      else if (char(c) == 'a')
	break;
    }

  cvDestroyAllWindows();
  //cvDestroyWindow("Face Tracker");
  cvReleaseCapture(&camera);
  return;
}


//==================================

/**turn terminal character echoing on or off*/
void echo( bool on = true )
{
  struct termios settings;
  tcgetattr( STDIN_FILENO, &settings );
  settings.c_lflag = on
    ? (settings.c_lflag |   ECHO )
    : (settings.c_lflag & ~(ECHO));
  tcsetattr( STDIN_FILENO, TCSANOW, &settings );
}


//=============================================================================
/**Ask the user if they are verifying or training
@return true if verifying.  false if training*/
static bool askUserIfVerifying()
{
  cout << "\nPlease enter 'v' for verification or 't' for training" << endl;
  string input;
  getline(cin, input);

  if (input != "v" && input != "t")
    return askUserIfVerifying();

  return input == "v";
}


/**turns on the webcam, asks the user to select a picture of themselves,
 computes and returns the features derived from the user's face*/
void getFeaturesFromWebcamFace(vector<double> &features)
{
  int argc = 1;
  const char ** argv;
  cv::Mat shape, visi;
  getPictureFromWebcam(shape, visi, argc, argv); //**** ignoring actual command line arguments if any *//
  features = generateFeatureVector(shape, visi);
}


/**verify the person sitting in front of the webcam*/
void runVerification()
{
  string userId, pass;
  cout << "\n please enter your user id and password for verification\n $username: ";
  getline(cin, userId);

  //--password
  cout << " $password: ";
  echo(false); //don't echo password to the terminal
  getline(cin, pass);
  echo(true); //turn terminal echo back on
  
  if (pass != userId)
    {
      cout << "\n invalid username / pass" << endl;
      return;
    }

  
  cout << "\n Username / Pass accepted\n==============\n" << endl;
  
  //-------on to face verification

  cout << "\n Going to VERIFY your face for userId = " << userId << "\n\n" << endl;
  vector<double> features;
  getFeaturesFromWebcamFace(features);


  if (!classifyIDSpecific(features, databaseFile, userId, 0.5)) //0.5 is the match threshold      
    cout << "\n\n\n ====\n\naccess denied: unknown user" << endl;
  else //must have iserId == idFromFace
    std::cout << "\n\n===\n you are " << userId << std::endl;
}

/**add images of the person sitting in front of the camera to the databse*/
void trainOnLiveUser()
{
  cout << "\n Going to DATABASE your face" << endl;

  vector<double> features;
  getFeaturesFromWebcamFace(features);

  //--ask user for name
  cout << "\n\n\n=========\n" << endl;
  std::string userId = "1", userConfirm = "2";
  while(userId != userConfirm)
    {
      cout << "\n please enter your user id and password\n $username: ";
      getline(cin, userId);
      cout << " $password: ";
      echo(false);
      getline(cin, userConfirm);
      echo(true);
    }

  //write to database
  addToDB(features, databaseFile, userId);
  cout << "\n added new " << userId << " face to database" << endl;
  
  //--see if the user wants to add more pictures to the databse
  string input = "";
  while (input != "t" && input != "q")
    {
      cout << "\n\n======\n Please enter 't' to continue training or 'q' to quit" << endl;
      getline(cin, input);
    }
  if (input == "t")
    trainOnLiveUser();
  
  //finished training  
  cout << "\n\n\n===========\n finished adding faces to the database" << endl;
}

void runTestFalsePositive(){
    const char * fv_db = "../db/lfw";
    std::ifstream training_set_file;
    training_set_file.open(fv_db);
    std::string line;
    
    std::vector<std::vector<double> > db_vectors;
    std::vector<std::string> db_names;
    
    // Parses text file, and generates corresponding feature_vectors
    while (training_set_file.good()) {
        getline(training_set_file,line);
        if (line.size() == 0) continue;
        char *cptr = new char[line.size()];
        std::strncpy(cptr, line.c_str(), line.size());
        
        char * pch;
        pch = std::strtok(cptr, ": ");
        db_names.push_back(std::string(pch));
        
        std::vector<double> single_vector;
        pch = std::strtok(NULL, ": ");
        while (pch != NULL) {
            single_vector.push_back(atof(std::string(pch).c_str()));
            pch = std::strtok(NULL, ": ");
        }
        db_vectors.push_back(single_vector);
    }
    cout << "Total attackers: " << db_vectors.size() << endl;
    int falsePositive = 0;
    string ids[4] = {"seojin", "mfleder", "jenn", "chris"};
    for(double threshold = 0.2; threshold < 5; threshold += 0.2){
      for(int i_id = 0; i_id < 4; i_id++){
	string userID = ids[i_id];
	for(int i = 0; i < db_vectors.size(); i++){
	  vector<double> test_vector = db_vectors[i];
          if(test_vector.size() != 132) continue;
	  string attacker = db_names[i];
	  //          cout << attacker << " ";
	  if(classifyIDSpecific(test_vector, string("../src/exe/data/training_vectors.txt"), userID, threshold)) falsePositive++;
         }
	cout<< "Threshold: " << threshold << " "<< userID <<" "<< falsePositive << " " << ((falsePositive + 0.0) / db_vectors.size() * 100) << endl;
      }
      cout << endl;
    }
    
}

 int main(int argc, const char** argv)
 {
   if (askUserIfVerifying())
       runVerification();  
   else //training
      trainOnLiveUser();

  //print 
  //for (int i = 0; i < features.size(); i++)
  //  std::cout << " " << features[i] << ", ";
  std::cout << std::endl;
    std::string input;
    getline(std::cin, input);  
  return 0;

  //runTestFalsePositive();
}
//=============================================================================
